import rclpy
from rclpy.node import Node
from std_msgs.msg import Float64, Float64MultiArray
from sensor_msgs.msg import JointState
from functools import partial

class SteeringBridge(Node):
    def __init__(self):
        super().__init__('steering_bridge')

        self.steer0 = 0.0
        self.steer1 = 0.0
        self.declare_parameter('ugv_ids', ['ugv1'])
        self.ugv_ids = self.get_parameter('ugv_ids').value
        self.pub = {}
        for ugv_id in self.ugv_ids:
            sub_topic = f'/world/default/model/{ugv_id}/joint_state'
            pub_topic = f'/{ugv_id}/actuators/chassis/out/steer_data'
            self.sub = self.create_subscription(
                JointState,
                sub_topic,
                partial(self.joint_state_callback, ugv_id=ugv_id),
                10
            )
        

            self.pub[ugv_id] = self.create_publisher(
                Float64MultiArray,
                pub_topic,
                10
            )


    def joint_state_callback(self,msg: JointState, ugv_id: str):
        idx0 = msg.name.index('servo_0')
        idx1 = msg.name.index('servo_1')
        self.steer0 = msg.position[idx0]
        self.steer1 = msg.position[idx1]

        # 构造输出消息
        out_msg = Float64MultiArray()
        out_msg.data = [self.steer0, self.steer1]

        self.pub[ugv_id].publish(out_msg)




def main(args=None):
    rclpy.init(args=args)
    node = SteeringBridge()
    rclpy.spin(node)
    node.destroy_node()
    rclpy.shutdown()

if __name__ == '__main__':
    main()

